#ifndef SYSTEMATICROBOT_H
#define SYSTEMATICROBOT_H

// Include files.
#include <set>
#include <map>
#include "WPILib.h"
#include "Subsystems/RobotSubsystem.hpp"
#include "Services/Service.hpp"
#include "Services/CompoundService.hpp"
#include "Services/Internal/GameService.hpp"

/**
 *  The SystematicRobot class represents a robot that holds all the subsystems and services and delegates to them.
 *  
 *  SystematicRobot is a cooler version of IterativeRobot, so the underlying control flow is similar.
 *  The robot will periodicly call (as part of it's periodic loop) the HandlePeriodic function in each awake RobotSubsystem and running Service that has been added to the robot.
 *  Most of your code will probably go in the HandlePeriodic functions.
 *  
 *  For understanding how work should be delegated to subsystems and service, consult the RobotSubsystem and Service pages.
 *  
 *  To use it, create a subclass place code in the constructor to add subsystems and services to your robot.
 *  Be sure to add your persistent, enabled, autonomous, and teleop services that will start and stop
 *  the other services when necessary.
 */
class SystematicRobot :
	public RobotBase
{
	friend class Subscriber;
	friend class GameService;
	
	public:
		static const double sDefaultPeriod;
		
		SystematicRobot();
		virtual ~SystematicRobot();
		void AddSubsystem(RobotSubsystem& subsystem);
		void AddSubsystem(RobotSubsystem* subsystem);
		void AddPersistentService(Service& service);
		void AddEnabledService(Service& service);
		void AddAutonomousService(Service &service);
		void AddTeleopService(Service &service);
		void AddTestService(Service &service);
		double GetPeriod();
		void SetPeriod(double period);
		double GetLoopsPerSec();
	protected:
		virtual void StartCompetition();
		virtual void Initialize();
	private:
		double m_period;
		Timer m_timer;
		CompoundService m_serviceRoot;
		CompoundService m_servicePersistant;
		Service m_serviceMechanisms;
		GameService m_serviceGame;
		std::map<void*, Subscriber*> m_controlLocks;
		
		void Run();
		bool GetIsNextPeriodReady();
		void DelayTask();
		void AddRootService(Service& service);
		Subscriber *GetControlLocker(void *obj);
		void *LockControl(Subscriber &subscriber, void *obj);
		void UnlockControl(Subscriber &subscriber, void *obj);
};
	
#endif // #ifndef SYSTEMATICROBOT_H 
